#include "cmn.h"


#include "../bmp/bmp.h"

unsigned char g_p_yuv444_l0[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];
unsigned char g_p_yuv444_l1[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];
unsigned char g_p_yuv444_l2[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];
unsigned char g_p_yuv444_copy_l0[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];
unsigned char g_p_yuv444_copy_l1[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];
unsigned char g_p_yuv444_copy_l2[IMG_MAX_SIZE * IMG_MAX_SIZE * 3];

static unsigned char g_p_ori_bitmap[IMG_MAX_SIZE*IMG_MAX_SIZE*3];
static unsigned char g_p_vga_bitmap[IMG_MAX_SIZE*IMG_MAX_SIZE*3];

static unsigned char g_p_image_a[IMG_MAX_SIZE*IMG_MAX_SIZE*3];
static unsigned char g_p_image_b[IMG_MAX_SIZE*IMG_MAX_SIZE*3];
static unsigned char g_p_image_c[IMG_MAX_SIZE*IMG_MAX_SIZE*3];



void yuv422_to_yuv444(unsigned char* p_yuv422, unsigned char* p_yuv444, int img_width, int img_height, int seg_422_bytewidth)
{
    unsigned char y, u, v;
    int X, Y;
    int line_width_444 = img_width * 3;
    for (Y = 0; Y < img_height; Y++) {
        for (X = 0; X < img_width; X++) {

            if (X%2) { 
                y = p_yuv422[Y*seg_422_bytewidth + 2*X + 1];
                u = p_yuv422[Y*seg_422_bytewidth + 2*(X - 1)];
                v = p_yuv422[Y*seg_422_bytewidth + 2*X];
            } else {
                y = p_yuv422[Y*seg_422_bytewidth + 2*X + 1];
                u = p_yuv422[Y*seg_422_bytewidth + 2*X];
                v = p_yuv422[Y*seg_422_bytewidth + 2*(X + 1)];
            }
            p_yuv444[3*X     + Y*line_width_444] = y;
            p_yuv444[3*X + 1 + Y*line_width_444] = u;
            p_yuv444[3*X + 2 + Y*line_width_444] = v;
        }
    }
}

#ifdef _HW_PRC_
void bitmap_to_yuv444(unsigned char* p_bitmap, int bitmap_width, int bitmap_height, int bitmap_linebyte, SEG_YUV444* p_yuv444)
{
	HW_IP_PRC prc_interface;
	int vga_width  = 640;
	int vga_height = (int)((float)bitmap_height*((float)vga_width/(float)bitmap_width));
	if (vga_height%2)
	{
		vga_height--;
	}

	resize_rgb(bitmap_linebyte, bitmap_width, bitmap_height, vga_width, vga_height, p_bitmap, g_p_vga_bitmap);
	rgb_to_yuv422(vga_width*3, 3, vga_width, vga_height, g_p_vga_bitmap, g_p_image_a);
            
	prc_interface.mode = 0;
	prc_interface.padding = 1;
	prc_interface.image_a.p_data    = g_p_image_a;
	prc_interface.image_a.width     = vga_width;
	prc_interface.image_a.height    = vga_height;
	prc_interface.image_a.bytewidth = vga_width*2;
	prc_interface.image_b.p_data    = g_p_image_b;
	prc_interface.image_c.p_data    = g_p_image_c;
	prc_interface.image_d.p_data    = 0;
	prc_interface.image_e.p_data    = 0;
	hw_ip_prc(&prc_interface);

	PRC_IMAGE seg_prc_img[3];
	seg_prc_img[0] = prc_interface.image_a;
	seg_prc_img[1] = prc_interface.image_b;
	seg_prc_img[2] = prc_interface.image_c;

	for (int i = 0; i < 3; ++i)
	{
	    int seg_img_width     = seg_prc_img[i].width;
	    int seg_img_height    = seg_prc_img[i].height;
	    int seg_422_bytewidth = seg_prc_img[i].bytewidth;
	    unsigned char* p_seg_422_img = seg_prc_img[i].p_data;
	    yuv422_to_yuv444(p_seg_422_img, p_yuv444[i].p_data, seg_img_width, seg_img_height, seg_422_bytewidth);
		p_yuv444[i].img_width  = seg_img_width;
		p_yuv444[i].img_height = seg_img_height;
	}
}

unsigned char* read_bmp(char* p_file_dir, int* p_img_width, int* p_img_height)
{
	int img_size = get_bmp_size(p_file_dir, p_img_width, p_img_height);
	get_bmp_rgb_data(p_file_dir, img_size, g_p_ori_bitmap);
	return g_p_ori_bitmap;
}
#endif

void clone_yuv444(SEG_YUV444* p_src, SEG_YUV444* p_dst, int touch_x, int touch_y, int start_layer, int end_layer)
{
	int qqvga_width  = p_src[2].img_width;
	int qqvga_height = p_src[2].img_height;

	int qvga_width   = p_src[1].img_width;
	int qvga_height  = p_src[1].img_height;
	
	int vga_width    = p_src[0].img_width;
	int vga_height   = p_src[0].img_height;

	//VGA
	p_dst[0].x0 = touch_x * 4 - (int)((float)qqvga_width/2.0f + 0.5f);
	p_dst[0].x1 = touch_x * 4 + (int)((float)qqvga_width/2.0f + 0.5f);
	p_dst[0].y0 = touch_y * 4 - (int)((float)qqvga_height/2.0f + 0.5f);
	p_dst[0].y1 = touch_y * 4 + (int)((float)qqvga_height/2.0f + 0.5f);

	if (p_dst[0].x0 < 0)
	{
		p_dst[0].x1 = p_dst[0].x1 - p_dst[0].x0;
		p_dst[0].x0 = 0;
	}
	else if (p_dst[0].x1 > vga_width - 1)
	{
		p_dst[0].x0 = p_dst[0].x0 - (p_dst[0].x1 - vga_width) - 1;
		p_dst[0].x1 = vga_width - 1;
	}

	if (p_dst[0].y0 < 0)
	{
		p_dst[0].y1 = p_dst[0].y1 - p_dst[0].y0;
		p_dst[0].y0 = 0;
	}
	else if (p_dst[0].y1 > vga_height - 1)
	{
		p_dst[0].y0 = p_dst[0].y0 - (p_dst[0].y1 - vga_height) - 1;
		p_dst[0].y1 = vga_height - 1;
	}
	p_dst[0].touch_x = touch_x * 4 - p_dst[0].x0;
	p_dst[0].touch_y = touch_y * 4 - p_dst[0].y0;

	//QVGA
	p_dst[1].x0 = touch_x * 2 - (int)((float)qqvga_width/2.0f + 0.5f);
	p_dst[1].x1 = touch_x * 2 + (int)((float)qqvga_width/2.0f + 0.5f);
	p_dst[1].y0 = touch_y * 2 - (int)((float)qqvga_height/2.0f + 0.5f);
	p_dst[1].y1 = touch_y * 2 + (int)((float)qqvga_height/2.0f + 0.5f);
	if (p_dst[1].x0 < 0)
	{
		p_dst[1].x1 = p_dst[1].x1 - p_dst[1].x0;
		p_dst[1].x0 = 0;
	}
	else if (p_dst[1].x1 > qvga_width - 1)
	{
		p_dst[1].x0 = p_dst[1].x0 - (p_dst[1].x1 - qvga_width) - 1;
		p_dst[1].x1 = qvga_width - 1;
	}

	if (p_dst[1].y0 < 0)
	{
		p_dst[1].y1 = p_dst[1].y1 - p_dst[1].y0;
		p_dst[1].y0 = 0;
	}
	else if (p_dst[1].y1 > qvga_height - 1)
	{
		p_dst[1].y0 = p_dst[1].y0 - (p_dst[1].y1 - qvga_height) - 1;
		p_dst[1].y1 = qvga_height - 1;
	}
	p_dst[1].touch_x = touch_x * 2 - p_dst[1].x0;
	p_dst[1].touch_y = touch_y * 2 - p_dst[1].y0;

	//QQVGA
	p_dst[2].x0 = 0;
	p_dst[2].x1 = qqvga_width;
	p_dst[2].y0 = 0;
	p_dst[2].y1 = qqvga_height;
	p_dst[2].touch_x = touch_x;
	p_dst[2].touch_y = touch_y;

	for (int i = start_layer; i < end_layer; ++i)
	{
		for (int x = 0; x < qqvga_width; x++)
		{
			for (int y = 0; y < qqvga_width; y++)
			{
				int dst_index = y * qqvga_width * 3 + x * 3;
				p_dst[i].p_data[dst_index + 0] = 0;
				p_dst[i].p_data[dst_index + 1] = 0;
				p_dst[i].p_data[dst_index + 2] = 0;
			}
		}
	}

	for (int i = start_layer; i < end_layer; ++i)
	{
		for (int x = p_dst[i].x0; x < p_dst[i].x1; x++)
		{
			for (int y = p_dst[i].y0; y < p_dst[i].y1; y++)
			{
				int src_index = y * p_src[i].img_width * 3 + x * 3;
				int dst_index = (y -p_dst[i].y0) * qqvga_width * 3 + (x - p_dst[i].x0) * 3;
				p_dst[i].p_data[dst_index + 0] = p_src[i].p_data[src_index + 0];
				p_dst[i].p_data[dst_index + 1] = p_src[i].p_data[src_index + 1];
				p_dst[i].p_data[dst_index + 2] = p_src[i].p_data[src_index + 2];
			}
		}
		p_dst[i].img_width  = qqvga_width;
		p_dst[i].img_height = qqvga_height;
	}
}

void access_by_24bit(int img_width, unsigned char* p_img, unsigned char* p_pixel, int x, int y)
{
	int line_byte = (img_width * 3 + 3) / 4 * 4;
	int index24bit = y * line_byte + x * 3;
	p_pixel[0] = p_img[index24bit + 0];
	p_pixel[1] = p_img[index24bit + 1];
	p_pixel[2] = p_img[index24bit + 2];
}

void set_by_24bit(int img_width, unsigned char* p_img, unsigned char* p_pixel, int x, int y)
{
	int line_byte = (img_width * 3  + 3) / 4 * 4;
	int index24bit = y * line_byte + x * 3;
	p_img[index24bit + 0] = p_pixel[0];
	p_img[index24bit + 1] = p_pixel[1];
	p_img[index24bit + 2] = p_pixel[2];
}

void access_by_8bit(int img_width, unsigned char* p_img, unsigned char* p_pixel, int x, int y)
{
	int line_byte = (img_width + 3) / 4 * 4;
	p_pixel[0] = p_img[y * line_byte + x];
}

void set_by_8bit(int img_width, unsigned char* p_img, unsigned char* p_pixel, int x, int y)
{
	int line_byte = (img_width + 3) / 4 * 4;
	p_img[y * line_byte + x ] = p_pixel[0];
}


void access_pixel(int img_width, unsigned char* p_img, int bitcount, unsigned char* p_pixel, int x, int y)
{
	if (bitcount == 24)
	{
		access_by_24bit(img_width, p_img, p_pixel, x, y);
	}
	else
	{
		access_by_8bit(img_width, p_img, p_pixel, x, y);
	}
}

void set_pixel(int img_width, unsigned char* p_img, int bitcount, unsigned char* p_pixel, int x, int y)
{
	if (bitcount == 24)
	{
		set_by_24bit(img_width, p_img, p_pixel, x, y);
	}
	else
	{
		set_by_8bit( img_width, p_img, p_pixel, x, y);
	}
}

float log2(float n)  
{  
    // log(n)/log(2) is log2.  
    return log(n) / log(2);  
}




void map_spix_avg_color_to_src_img(S_Pix* p_spix, short* p_label_map, unsigned char* p_src_img, int width, int height)
{
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int si = p_label_map[y * width + x];
			p_src_img[y * width * 3 + x * 3 + 0] =  p_spix[si].color.c0;
			p_src_img[y * width * 3 + x * 3 + 1] =  p_spix[si].color.c1;
			p_src_img[y * width * 3 + x * 3 + 2] =  p_spix[si].color.c2;
		}
	}
}

void map_spix_saliency_to_src_img(S_Pix* p_spix, short* p_label_map, unsigned char* p_src_img, int width, int height)
{
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int si = p_label_map[y * width + x];
			//p_src_img[y * width * 3 + x * 3 + 0] =  p_spix[si].salient_conf;
			//p_src_img[y * width * 3 + x * 3 + 1] =  p_spix[si].salient_conf;
			//p_src_img[y * width * 3 + x * 3 + 2] =  p_spix[si].salient_conf;
			p_src_img[y * width * 3 + x * 3 + 0] =  p_spix[si].color.c3;
			p_src_img[y * width * 3 + x * 3 + 1] =  p_spix[si].color.c3;
			p_src_img[y * width * 3 + x * 3 + 2] =  p_spix[si].color.c3;
		}
	}
}


void map_af_to_src_img(S_Pix* p_spix, short* p_label_map, unsigned char* p_src_img, int width, int height)
{
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int si = p_label_map[y * width + x];
			p_src_img[y * width * 3 + x * 3 + 0] =  p_spix[si].salient_conf;
			p_src_img[y * width * 3 + x * 3 + 1] =  p_spix[si].salient_conf;
			p_src_img[y * width * 3 + x * 3 + 2] =  p_spix[si].salient_conf;
		}
	}
}

void zero_background(S_Pix* p_spix, short* p_label_map, unsigned char* p_src_img, int width, int height)
{
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int si = p_label_map[y * width + x];
			if (p_spix[si].is_bg)
			{
				p_src_img[y * width * 3 + x * 3 + 0] =  0;
				p_src_img[y * width * 3 + x * 3 + 1] =  0; //128
				p_src_img[y * width * 3 + x * 3 + 2] =  0; //128
			}
		}
	}
}

void zero_foreground(S_Pix* p_spix, short* p_label_map, unsigned char* p_src_img, int width, int height)
{
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int si = p_label_map[y * width + x];
			if (p_spix[si].is_bg == false)
			{
				p_src_img[y * width * 3 + x * 3 + 0] =  0;
				p_src_img[y * width * 3 + x * 3 + 1] =  0; //128
				p_src_img[y * width * 3 + x * 3 + 2] =  0; //128
			}
		}
	}
}

//=================================================================================
/// DrawContoursAroundSegments
///
/// Internal contour drawing option exists. One only needs to comment the if
/// statement inside the loop that looks at neighbourhood.
//=================================================================================
void draw_contour_around_segments(sp_output* p_s_evaluate, unsigned char* p_dst_img, int img_width, int img_height)
{
	int x = 0, y = 0;
	unsigned char p_pixel[3] = {0};
	const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};
	const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};

	int sz = img_width * img_height;
	vector<bool> istaken(sz, false);

	if (p_s_evaluate->boundary_ok == false)
	{
		int mainindex(0);
		for( int j = 0; j < img_height; j++ )
		{
			for( int k = 0; k < img_width; k++ )
			{

				int np(0);
				p_s_evaluate->p_edge_map[mainindex] = 0;
				for( int i = 0; i < 8; i++ )
				{
					int x = k + dx8[i];
					int y = j + dy8[i];

					if( (x >= 0 && x < img_width) && (y >= 0 && y < img_height) )
					{
						int index = y * img_width + x;

						if( false == istaken[index] )//comment this to obtain internal contours
						{
							if( p_s_evaluate->p_s_pix_map[mainindex] != p_s_evaluate->p_s_pix_map[index] ) np++;
						}
					}
				}
				if( np > 1 )//change to 2 or 3 for thinner lines
				{
					p_s_evaluate->p_edge_map[mainindex] = 255;
					istaken[mainindex]    = true;
				}
				mainindex++;
			}
		}
		p_s_evaluate->boundary_ok = false;
	}


	if (p_dst_img != NULL)
	{
		//p_pixel[0] = 255;
		//p_pixel[1] = 128;
		//p_pixel[2] = 128;

		p_pixel[0] = 255;
		p_pixel[1] = 255;
		p_pixel[2] = 255;
		for (y = 0; y < img_height; y++)
		{
			for (x = 0; x < img_width; x++)
			{
				if (p_s_evaluate->p_edge_map[y * img_width + x] > 0)
				{
					set_pixel(img_width, p_dst_img, 24, p_pixel, x, y);
				}
			}
		}
	}
}

void draw_spix_center_point(S_Pix* p_spix, int valid_s_pix_num, unsigned char* p_dst_img, int img_width, int img_height)
{
	unsigned char p_pixel[3];
	p_pixel[0] = 0;
	p_pixel[1] = 255;
	p_pixel[2] = 0;
	const int dx8[9] = {-1, -1,  0,  1, 1, 1, 0, -1, 0};
	const int dy8[9] = { 0, -1, -1, -1, 0, 1, 1,  1, 0};
	for (int i = 0; i < valid_s_pix_num; i++)
	{
		int x =  p_spix[i].X;
		int y =  p_spix[i].Y;
		for(int j = 0; j< 9; j+=2 )
		{
			int new_x = x + dx8[j];
			int new_y = y + dy8[j];

			if( (new_x >= 0 && new_x < img_width) && (new_y >= 0 && new_y < img_height) )
			{
				set_pixel(img_width, p_dst_img, 24, p_pixel, new_x, new_y);
			}
		}
	}
}


void fill_color_for_spix_by_src_img(short* p_s_pix_map, unsigned char* p_src_map, int width, int height, S_Pix* p_spix, int valid_s_pix_num)
{
	int x, y, si;
	int c0, c1, c2;
	int index_24bit = 0;
	int line_byte24bit = (width * 3 + 3) / 4 * 4;

	for (si = 0; si < valid_s_pix_num; si++)
	{
		p_spix[si].size     = 1;
		p_spix[si].color.c0 = 0;
		p_spix[si].color.c1 = 0;
		p_spix[si].color.c2 = 0;
		p_spix[si].X = 0;
		p_spix[si].Y = 0;

	}

	for (y = 0; y < height; y++)
	{
		for (x = 0; x < width; x++)
		{
			si = p_s_pix_map[y * width + x];
			index_24bit = y * line_byte24bit + x * 3;
			p_spix[si].color.c0 += p_src_map[index_24bit + 0];
			p_spix[si].color.c1 += p_src_map[index_24bit + 1];
			p_spix[si].color.c2 += p_src_map[index_24bit + 2];
			p_spix[si].X += x;
			p_spix[si].Y += y;
			p_spix[si].size++;
		}
	}

	for (si = 0; si < valid_s_pix_num; si++)
	{
		p_spix[si].color.c0 = (int)((float)p_spix[si].color.c0 / (float)p_spix[si].size + 0.5f);
		p_spix[si].color.c1 = (int)((float)p_spix[si].color.c1 / (float)p_spix[si].size + 0.5f);
		p_spix[si].color.c2 = (int)((float)p_spix[si].color.c2 / (float)p_spix[si].size + 0.5f);
		p_spix[si].X = (int)((float)p_spix[si].X / (float)p_spix[si].size + 0.5f);
		p_spix[si].Y = (int)((float)p_spix[si].Y / (float)p_spix[si].size + 0.5f);
	}
}



void draw_line(unsigned char* p_src_map, int width, int height, int x0, int y0, int x1, int y1, unsigned char* p_color)
{
	int x, y;
	int line_byte24bit = (width * 3 + 3) / 4 * 4;
	for (y = y0; y <= y1; y++)
	{
		for (x = x0; x <= x1; x++)
		{
			int index_24bit = y * line_byte24bit + x * 3;
			p_src_map[index_24bit + 0] = p_color[0];
			p_src_map[index_24bit + 1] = p_color[1];
			p_src_map[index_24bit + 2] = p_color[2];
		}
	}
}


void draw_box(unsigned char* p_src_map, int width, int height, short* p_win, unsigned char* p_color)
{
	draw_line(p_src_map, width, height, p_win[0], p_win[1], p_win[2], p_win[1], p_color);
	draw_line(p_src_map, width, height, p_win[0], p_win[1], p_win[0], p_win[3], p_color);
	draw_line(p_src_map, width, height, p_win[2], p_win[1], p_win[2], p_win[3], p_color);
	draw_line(p_src_map, width, height, p_win[0], p_win[3], p_win[2], p_win[3], p_color);
}


void draw_surround_win(unsigned char* p_src_map, int width, int height, S_Pix* p_spix, int valid_s_pix_num, short* p_final_win)
{
	unsigned char p_color[3];

	for (int si = 0; si < valid_s_pix_num; si++)
	{
		//if (p_spix[si].is_fg == true)
		/*if (p_spix[si].is_bg == false)
		{
			p_color[0] = 0;
			p_color[1] = 255;
			p_color[2] = 0;
			draw_box(p_src_map, width, height, p_spix[si].win, p_color);

			short win[4] = {0};
			win[0] =  p_spix[si].win[0] + 1;
			win[1] =  p_spix[si].win[1] + 1;
			win[2] =  p_spix[si].win[2] - 1;
			win[3] =  p_spix[si].win[3] - 1;
			p_color[0] = 0;
			p_color[1] = 0;
			p_color[2] = 0;
			draw_box(p_src_map, width, height, win, p_color);
		}*/
	}

	p_color[0] = 0;
	p_color[1] = 255;
	p_color[2] = 0;
	draw_box(p_src_map, width, height, p_final_win, p_color);

	short win[4] = {0};
	win[0] = p_final_win[0] + 1;
	win[1] = p_final_win[1] + 1;
	win[2] = p_final_win[2] - 1;
	win[3] = p_final_win[3] - 1;
	p_color[0] = 0;
	p_color[1] = 0;
	p_color[2] = 0;
	draw_box(p_src_map, width, height, win, p_color);
	
}


void draw_segmentation_result(sp_output* p_s_evaluate, int valid_s_pix_num, unsigned char* p_result_img, int img_width, int img_height, 
					 bool draw_contour, bool fill_avg_color, bool draw_center, bool draw_win, bool zero_back, bool zero_fore, bool fill_saliency)
{
	//fill_color_for_spix_by_src_img(p_s_evaluate->p_s_pix_map, p_result_img, img_width, img_height, p_s_evaluate->p_spix, valid_s_pix_num);

	if (fill_avg_color)
	{
		map_spix_avg_color_to_src_img(p_s_evaluate->p_spix, p_s_evaluate->p_s_pix_map, p_result_img, img_width, img_height);
	}

	if (fill_saliency)
	{
		map_spix_saliency_to_src_img(p_s_evaluate->p_spix, p_s_evaluate->p_s_pix_map, p_result_img, img_width, img_height);
	}

	if (zero_back)
	{
		zero_background(p_s_evaluate->p_spix, p_s_evaluate->p_s_pix_map, p_result_img, img_width, img_height);
	}

	if (zero_fore)
	{
		zero_foreground(p_s_evaluate->p_spix, p_s_evaluate->p_s_pix_map, p_result_img, img_width, img_height);
	}

	if (draw_center)
	{
		draw_spix_center_point(p_s_evaluate->p_spix, valid_s_pix_num, p_result_img, img_width, img_height);
	}



	if (draw_contour)
	{
		draw_contour_around_segments(p_s_evaluate, p_result_img, img_width, img_height);
	}

	
	if (draw_win)
	{
		draw_surround_win(p_result_img, img_width, img_height, p_s_evaluate->p_spix, valid_s_pix_num, p_s_evaluate->final_win);
	}
	

}